\documentclass[UTF8]{ctexbook}
\usepackage{amsmath}
\usepackage{geometry}
\usepackage{bm}
\usepackage{caption}
\usepackage{subcaption}
\usepackage{mathtools}
\usepackage{xfrac}
\usepackage{xcolor} 
\usepackage{tikz} 
\usetikzlibrary{arrows,shapes,chains}
\geometry{left=3.18cm,right=3.18cm,top=2.54cm,bottom=2.54cm}
\usepackage{graphicx}
\pagestyle{plain}
\usepackage{setspace}

\date{}
\CTEXsetup[number={\chinese{section}}]{section}

\begin{document}
	\begin{center}
		\quad \\
		\quad \\
		\quad \\
		\songti \zihao{2}  \textbf{中\quad 国\quad 舰\quad 船\quad 研\quad 究\quad 院} \\
		硕\ \ 士\ \ 学\ \ 位\ \ 论\ \ 文
	\end{center}
	\vskip 1.5cm
	
	\begin{quotation}
		\songti \zihao{4}
		\par\setlength\parindent{3em}
		\quad 
		
		题\hspace{4em}目：\underline{\hspace{1.55em}基于视觉定位的水下液压机 \hspace{1.55em}}

		\hspace{6.7em} \underline{\hspace{3.35em} 械臂运动控制研究 \hspace{3.35em}}
		
		研\hspace{0.25em}究\hspace{0.25em}生\hspace{0.25em}姓\hspace{0.25em}名：\underline{\hspace{5.9em} 杨茂洲 \hspace{5.9em}}
		
		专\hspace{4em}业：\underline{\hspace{1.55em}船舶与海洋结构物设计制造 \hspace{1.55em}}
		
		导\hspace{4em}师：\underline{\hspace{5.9em} 谢基榕 \hspace{5.9em}}
		
		培\hspace{0.667em}养\hspace{0.667em}单\hspace{0.667em}位：\underline{\hspace{2.4em} 中国船舶科学研究中心 \hspace{2.4em}}
		
		\vskip 3.5cm
		\centering
		2020年11月25日
	\end{quotation}
	\newpage

\tableofcontents
\mainmatter

\chapter{前言}
略
	
\chapter{运动学分析}
\section{正运动学计算}
略

\section{逆运动学计算} 

已知机械臂末端执行器位姿，求解各关节角度这一过程称为逆运动学问题。Pieper证明了具有相邻三根关节轴交于一点的六自由度机械手具有封闭的解析解，Duffy证明了有相邻三根关节轴平行的六自由度机械手具有封闭的解析解。对于满足Pipper,Duff准则的机械臂而言，运用解析法求解运动学逆解比较简单。而对于其他不满足Pipper,Duff准则的机械臂，数值解法便是唯一选择。其中牛顿-拉夫森迭代法简单易实现，应用广泛。

根据机械臂的DH参数和连杆坐标系，4自由度的正向运动学方程为：

\begin{equation}
\bm{T}_{end}=
	\bm{T}_1\ \bm{T}_2\ \bm{T}_3\ \bm{T}_4=
	\bm{T}_{\theta}(\theta_1,\theta_2,\theta_3,\theta_4)
\label{kinematics}
\end{equation}

其中$\bm{T}_i=Trans_z(d_i)Rot_z(\theta_i)Trans_x(a_i)Rot_x(\alpha_i) (i=1,2,3,4)$，$\alpha_i$、$\theta_i$、$d_i$、$a_i$为DH参数，$\bm{T}_{end}$为机械臂末端位姿的变换矩阵

用包含六自由度信息的运动矢量来表达机械臂的末端位姿：

\begin{equation} 
\bm{f}=
	\begin{bmatrix} 
		p_x \\ p_y \\ p_z \\ \hat{\theta} \\ \psi \\ \phi 
	\end{bmatrix} 
\label{运动矢量}
\end{equation}

其中$p_x$、$p_y$、$p_z$为沿坐标轴x、y、z的平移量，位置矢量
\begin{equation} 
\bm{P}=
	\begin{bmatrix} 
		p_x \\ p_y \\ p_z 
	\end{bmatrix}
\end{equation}

$\hat{\theta}$、$\psi$、$\phi$为绕坐标轴x、y、z的旋转角度(为避免与关节转角$\theta$相混淆，绕x轴的转角用$\hat{\theta}$表示)，对应的旋转矩阵分别为：
\begin{equation} 
\bm{R}(x,\hat{\theta})=
	\begin{bmatrix} 
		1 & 0 & 0 \\ 
		0 & \cos\hat{\theta} & -\sin\hat{\theta} \\ 
		0 & \sin\hat{\theta} & \cos\hat{\theta} 
	\end{bmatrix}
\end{equation}

\begin{equation} 
\bm{R}(y,\psi)=
	\begin{bmatrix} 
		\cos\psi & 0 & \sin\psi \\ 
		0 & 1 & 0 \\ 
		-\sin\psi & 0 & \cos\psi 
	\end{bmatrix} 
\end{equation}

\begin{equation} 
\bm{R}(z,\phi)=
	\begin{bmatrix} 
		\cos\phi & -\sin\phi & 0 \\
		\sin\phi & \cos\phi & 0 \\
		0 & 0 & 1 
	\end{bmatrix} 
\end{equation}

绕任意轴旋转角度$\theta$的旋转矩阵可等价为上述三个旋转矩阵的乘积

令$c_1=\cos\hat{\theta}$，$s_1=\sin\hat{\theta}$，
$c_2=\cos\psi$，$s_2=\sin\psi$，
$c_3=\cos\phi$，$s_3=\sin\phi$

\begin{equation} 
\bm{R}=
	\bm{R}(x,\hat{\theta})\cdot \bm{R}(y,\psi)\cdot \bm{R}(z,\phi)=
		\begin{bmatrix} 
			c_2c_3 & -c_2s_3 & s_2 \\
			c_1s_3+c_3s_1s_2 &  c_1c_3-s_1s_2s_3 & -c_2s_1 \\
			s_1s_3-c_1c_3 s_2 & c_3s_1+c_1s_2s_3 & c_1c_2 
		\end{bmatrix}
\end{equation}

运动矢量与变换矩阵的转化关系为：
\begin{equation} 
\bm{T}=
	\begin{bmatrix} 
		\bm{R} & \bm{P} \\ 0 & 1 
	\end{bmatrix}
\end{equation}

根据等号两边对应元素相等可得：

$$ p_x=\bm{T}_{14} $$
$$ p_y=\bm{T}_{24} $$
$$ p_z=\bm{T}_{34} $$
$$ \hat{\theta}=atan2(-\bm{T}_{23},\bm{T}_{33}) $$
$$ \psi=arcsin(\bm{T}_{13}) $$
$$ \phi=atan2(-\bm{T}_{12},\bm{T}_{11}) $$

其中$\bm{T}_{ij}$表示变换矩阵中第i行，第j列的元素；atan为编程语言中的常用函数，其定义如下：

$$ atan(x,y)=\begin{cases} 
\displaystyle\arctan\frac{y}{x}\qquad  & x>0 \\ \\
\displaystyle\arctan\frac{y}{x}+\pi\qquad & y\ge 0,x<0 \\ \\
\displaystyle\arctan\frac{y}{x}-\pi\qquad & y<0,x<0 \\ \\
+\displaystyle\frac{\pi}{2}\qquad & y>0,x=0 \\ \\
-\displaystyle\frac{\pi}{2}\qquad & y<0,x=0 \\ \\
undefined\qquad & y=0,x=0
\end{cases} $$

雅克比矩阵定义如下：
\begin{equation}
J=
	\left[\frac{\partial f_i(\theta)}{\partial \theta_j}\right]= 
		\begin{bmatrix}
 			\frac{\partial p_x(\theta)}{\partial \theta_1} &
 			\frac{\partial p_x(\theta)}{\partial \theta_2} & 
 			\frac{\partial p_x(\theta)}{\partial \theta_3} & 
 			\frac{\partial p_x(\theta)}{\partial \theta_4} \\ 
 			\frac{\partial p_y(\theta)}{\partial \theta_1} & 
 			\frac{\partial p_y(\theta)}{\partial \theta_2} & 
 			\frac{\partial p_y(\theta)}{\partial \theta_3} & 
 			\frac{\partial p_y(\theta)}{\partial \theta_4} \\ 
 			\frac{\partial p_z(\theta)}{\partial \theta_1} & 
 			\frac{\partial p_z(\theta)}{\partial \theta_2} & 
 			\frac{\partial p_z(\theta)}{\partial \theta_3} & 
 			\frac{\partial p_z(\theta)}{\partial \theta_4} \\ 
 			\frac{\partial \alpha(\theta)}{\partial \theta_1} & 
 			\frac{\partial \alpha(\theta)}{\partial \theta_2} & 
 			\frac{\partial \alpha(\theta)}{\partial \theta_3} & 
 			\frac{\partial \alpha(\theta)}{\partial \theta_4} \\ 
 			\frac{\partial \beta(\theta)}{\partial \theta_1} & 
 			\frac{\partial \beta(\theta)}{\partial \theta_2} & 
 			\frac{\partial \beta(\theta)}{\partial \theta_3} & 
 			\frac{\partial \beta(\theta)}{\partial \theta_4} \\ 
 			\frac{\partial \gamma(\theta)}{\partial \theta_1} & 
 			\frac{\partial \gamma(\theta)}{\partial \theta_2} & 
 			\frac{\partial \gamma(\theta)}{\partial \theta_3} & 
 			\frac{\partial \gamma(\theta)}{\partial \theta_4} 
 		\end{bmatrix}
\end{equation}

按照这种方式建立雅克比矩阵的好处在于雅克比矩阵中的每一个元素都具有明确的物理意义，即机械臂末端执行器速度（角速度）与各关节角速度的“广义传动比”。此处的雅可比矩阵由于不是方阵，需要使用广义逆求其最小二乘解。

得到雅克比矩阵之后，便可通过牛顿-拉夫森迭代公式计算关节转角：
\begin{equation} 
\theta_{n+1}=
	\theta_n-{(J^TJ)}^{-1}J^T\Delta f(\theta_n)
\end{equation}

逆运动学计算步骤如下：

1.指定当前运动矢量的真实值$\bm{f}_{tar}$（通过视觉系统测量得到）,设置容许误差$\epsilon$和最大迭代次数$N$,估计一个关节矢量$\bm{\theta}_{init}$通过运动学正解得到当前位姿变换矩阵的估计值$\bm{T}_{cur}$,进而计算出运动矢量的估计值$\bm{f}_{cur}$作为迭代初值输入。

2.根据当前运动矢量的估计值$\bm{f}_{cur}$和真实值$\bm{f}_{tar}$计算微分运动矩阵$\Delta \bm{f}(\bm{\theta}_n)$，进而得到偏差$error=\Delta \bm{f}(\bm{\theta})\cdot (\Delta \bm{f}(\bm{\theta}))^T$

3.当迭代次数大于N时，表明运动学逆解不存在。当迭代次数小于N时，若$error<\epsilon$，迭代结束，输出$\bm{\theta}_{cur}$；若$error>\epsilon$，根据迭代公式计算关节角增量，更新当前关节矢量$\bm{\theta}_{cur}$,重复步骤2和步骤3。

详细代码实现参见文件：kinematics.py

\section{直线轨迹插补}

轨迹规划既可在笛卡尔空间进行，也可在关节空间中进行。在笛卡尔空间进行轨迹规划直观易理解，往往更容易满足工程中对于特定轨迹和避障的要求。

轨迹的生成一般是先给定轨迹的起始点与终止点的位姿信息，根据轨迹插补算法计算出插补点的位姿信息，将机械臂跟踪连续轨迹运动转化为插补点之间“点到点”的运动。对于工程上常见的直线轨迹，需采用抛物线过渡的线性函数（即梯形加减速）来保证整段轨迹上的位移和速度都连续。

假定机械臂末端执行器由$P1$点沿直线轨迹移动到$P2$点。$(x_1,y_1,z_1)$和$(\hat{\theta}_1,\psi_1,\phi_1)$为$P1$点的位置矢量和旋转矢量，$(x_2,y_2,z_2)$和$(\hat{\theta}_2,\psi_2,\phi_2)$为$P2$点的位置矢量和旋转矢量，$(x_i,y_i,z_i)$和$(\hat{\theta}_i,\psi_i,\phi_i)$为插补点的位置矢量和旋转矢量。

各个插补点的运动矢量可表示为：
\begin{equation}
\bm{f}_i=
	\bm{f}_1+\lambda\Delta\bm{f}
\end{equation}

其中运动矢量$\bm{f}$定义参见式\ref{运动矢量}，$\lambda$为归一化因子，$\Delta\bm{f}$表示首末点之间运动矢量的增量，其求解为：
\begin{equation}
\Delta\bm{f}=
	\bm{f}_2-\bm{f}_1
\end{equation}

基于抛物线过渡(梯形加减速)的空间直线插补的核心在于归一化参数$\lambda$的计算，需要加入速度规划的物理条件和约束。此得到的直线插补算法需要初始化的参数有机械臂末端线速度$v_s$,加速度$a_a$，减速度$a_d$，插值时间间隔$\Delta t$。需要计算得到的插值参数有总位移S和插值点数N。

对于空间直线插补而言，总位移计算公式如下：
\begin{equation}
S=
	\sqrt{
		(x_1-x_2)^2+(y_1-y_2)^2+(z_1-z_2)^2
	}
\end{equation}

加速段，减速段用时和位移分别为
\begin{equation}
T_1=
	\frac{v_s}{a_a}
\end{equation}

\begin{equation}
T_2=
	\frac{v_s}{a_d}
\end{equation}

\begin{equation}
S_1=
	\frac{1}{2}v_sT_1
\end{equation}

\begin{equation}
S_2=
	\frac{1}{2}v_sT_2
\end{equation}

沿轨迹运动总时间为
\begin{equation}
T=
	T_1+T_2+\frac{S-S_1-S_2}{v_s}
\end{equation}

插补数
\begin{equation}
N=
	\frac{T}{\Delta t} 
\end{equation}

将位移、速度和加速度进行归一化处理得到上述计算量的归一化参数
\begin{equation}
S_{1\lambda}=
	\frac{S_1}{S}
\end{equation}

\begin{equation}
T_{1\lambda}=
	\frac{T_1}{T}
\end{equation}

\begin{equation}
T_{2\lambda}=
	\frac{T_2}{T}
\end{equation}

\begin{equation}
a_{a\lambda}=
	\frac{2S_{1\lambda}}{T^2_{1\lambda}}
\end{equation}

\begin{equation}
a_{d\lambda}=
	\frac{2S_{2\lambda}}{T^2_{2\lambda}}
\end{equation}

结合梯形加减速的位移公式，可以推导出归一化因子$\lambda$的计算公式如下：
\begin{equation}
\lambda=
	\begin{cases} 
		\frac{1}{2}a_{a\lambda}t^2 \qquad & 
		
		(0\leq t\leq T_{1\lambda}) \\
		 
		\frac{1}{2}a_{a\lambda}T^2_{1\lambda}+a_{a\lambda}T_{1\lambda}(t-T_{1\lambda}) \qquad & 
		(T_{1\lambda}<t\leq 1-T_{2\lambda}) \\ 
		
		\frac{1}{2}a_{a\lambda}T^2_{1\lambda}+a_{a\lambda}T_{1\lambda}(1-T_{1\lambda}-T_{2\lambda})+\frac{1}{2}a_{d\lambda}{(t+T_{2\lambda}-1)}^2 \qquad & 
		
		(1-T_{2\lambda}<t\leq 1) 
	\end{cases}
\end{equation}

其中t表示时间，$t=i/N(0\leq t\leq 1)$,$i=1,2,3,\ldots,N$。

详细代码实现参见文件：path-planning.py

\chapter{液压系统建模}
\section{比例放大器数学模型}

由于数据采集板卡的输出模式为电压输出，而比例阀中的比例电磁铁需要由电流驱动，因此需要利用功率放大器对控制信号进行放大，将电压转换为电流，进而驱动比例阀正常工作。

由于比例放大器的转折频率远高于系统的频宽，因此可将比例放大器试做一个比例环节。其传递函数为：
\begin{equation}
G_a(s)=
	\frac{I(s)}{U(s)}=
		K_a
\end{equation}

$I$为输出电流。

$U$为输入电压。

$K_a$为比例增益。

\section{电液比例阀数学模型}

电液比例阀兼具方向阀和流量阀的功能，能够通过输入电流来驱动比例电磁铁，进而控制阀芯运动来改变流经阀口的流量大小和方向。一般情况下可将其简化为一个二阶系统，其数学模型如下所示：
\begin{equation}
G_v(s)=
	\frac{X_v(s)}{I(s)}=
		\frac{K_v}{\displaystyle \frac{s^2}{\omega_v^2}+\frac{2\xi_v}{\omega_v}s+1}
\end{equation}

$x_v$为比例阀阀芯位移。

$I$为比例放大器输出电流。

$K_v$为比例电磁铁的增益系数。

$\omega_v$为比例电磁铁的固有频率。

$\xi_v$为比例电磁铁的阻尼比。\\

对阀口流量方程进行泰勒级数展开，将其线性化为：
\begin{equation}
q_L=
	K_xx_v+K_pp_L 	
	\label{阀口流量方程}
\end{equation}

$q_L$为负载流量。

$p_L$为负载压力。

$K_x$为比例阀流量系数。

$K_p$为比例阀压力-流量系数。\\

令$K_{sv}=K_xK_v$，则可推导出比例阀输入的电流信号与输出的流量信号间的关系：
\begin{equation}
\frac{Q_L(s)}{I(s)}=
	\frac{K_{sv}}{\displaystyle \frac{s^2}{\omega_v^2}+\frac{2\xi_v}{\omega_v}s+1}
\end{equation}1*4

$K_{sv}$为比例阀对于输入电流的流量增益系数。\\

\section{液压缸-负载数学模型}

本文研究的机械臂摆动关节采用的“HLK-1300”型号液压缸属于非对称液压缸。图$\ref{1}$给出比例阀控液压缸系统的结构简图。其中$A_1、A_2$分别为液压缸无杆腔和有杆腔的横截面积;$p_1，p_2$分别为液压缸无杆腔和有杆腔的油液压力；$q_1，q_2$分别为液压缸无杆腔和有杆腔的流量；$p_s$为供油压力，保持不变；$p_0$为回油压力，近似为0；$x_v$为比例阀阀芯位移；$x$为液压缸活塞位移。下面将以活塞杆外伸的情况建立数学模型。

\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.7\textwidth]{figure/Fig1.png}
	\caption{比例阀控液压缸系统结构简图} 
	\label{1}
\end{figure*}

忽略管道摩擦损失和流体质量影响；假定油液温度、体积弹性模量为常数，液压缸的内外泄露为层流流动；定义负载压力$p_L$和负载流量$q_L$如下所示：
\begin{equation}
	\begin{cases} 
		p_L=
			p_1-np_2\qquad (n=\frac{A_1}{A_2}) \\ 
		q_L=
			q_1 
	\end{cases} 
\end{equation}

根据可压缩流体的流量连续性方程，结合液压缸的载荷特性可得：
\begin{equation}
q_L=
	C_{tp}p_L+A_1\frac{dx}{dt}+\frac{V_e}{4\beta_e}\frac{dp_L}{dt} 	
\label{流量连续性方程}
\end{equation}

$C_{tp}$为总泄露系数。

$\beta_e$为液压油的弹性系数。

$V_e$为液压缸的等效容积,有$ \displaystyle V_e=\frac{4V_1}{1+n^3} $。\\

为便于系统建模分析，液压油与负载质量组成的系统可等效为一个“质量-弹簧-阻尼”的二阶震荡系统。忽略库伦摩擦等非线性负载和油液质量，根据牛顿第二定理可得液压缸-负载的力平衡方程通式如下所示：
\begin{equation}
p_1A_1-p_2A_2=
	p_LA_1=
		m_t\frac{d^2x}{dt^2}+B_p\frac{dx}{dt}+K_ex+F_L 	
\label{力平衡方程}
\end{equation}

$m_t$为负载质量。

$B_P$为负载的阻尼系数。

$K_e$为负载的弹性系数。

$F_L$为负载力。\\

对\ref{阀口流量方程}、\ref{流量连续性方程}、\ref{力平衡方程}式进行拉式变换后联立得到比例阀控非对称液压缸位移的数学模型：
\begin{equation}
X(s)=
	\frac{
		\displaystyle K_xX_v(s)-\frac{K_t}{A_1}(1+\frac{V_es}{4\beta_eK_t})F_L(s)
	}{
		\displaystyle \frac{V_em_ts^3}{4\beta_eA_1}+
		(\frac{m_tK_t}{A_1}+
		\frac{V_eB_p}{4\beta_eA_1})s^2+
		(\frac{B_pK_t}{A_1}+\frac{V_eK_e}{4\beta_eA_1}+A_1)s+
		\frac{K_tK_e}{A_1}
	} 	
\label{拉氏变换}
\end{equation}

其中系数$K_t=C_{tp}+K_p$。在位置控制系统中，只存在惯性负载，即$K_e=0$。由于$K_t$和$B_p$都很小，$B_pK_t/A_1\ll 1$，可忽略不计。于是式\ref{拉氏变换}可化简为：

\begin{equation}
X(s)=
	\frac{
		\displaystyle\frac{K_xX_v(s)}{A_1}-
		(K_t+\frac{V_e}{4\beta_e}s)\frac{F_L(s)}{A_1^2}
	}{
		\displaystyle\frac{m_tV_e}{4\beta_eA_1^2}s^3+
		\frac{m_tK_t}{A_1^2}s^2+s
	} 
\label{拉氏变换简化}
\end{equation}

令液压系统的固有频率$\displaystyle\omega_h=\sqrt{\frac{4\beta_eA_1^2}{m_tV_e}}$，液压系统阻尼比$\displaystyle\xi_h=\frac{K_t}{A_1}\sqrt{\frac{\beta_em_t}{V_e}}$，则式\ref{拉氏变换简化}可进一步化简为：

\begin{equation}
X(s)=
	\frac{
		\displaystyle \frac{K_xX_v(s)}{A_1}-
		(K_t+\frac{V_e}{4\beta_e}s)\frac{F_L(s)}{A_1^2}
	}{
		\displaystyle s(\frac{s^2}{\omega_h^2}+
		\frac{2\xi_hs}{\omega_h}+1)
	}
\end{equation}

\section{液压系统数学模型}

结合上述各部分推导出的传递函数，可得到比例阀控非对称液压缸系统的传递函数框图如图$\ref{2}$所示：

\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.85\textwidth]{figure/Fig2.png}
	\caption{系统传递函数框图}
	\label{2}
\end{figure*}

整个系统的输入为相机识别的目标点期望位姿，反馈量为相机识别的机械臂当前实际位姿。自动化改造的第一步便是使机械臂能够根据输入信号，做出相应动作，即让机械臂能够“动起来”。换而言之便是暂不考虑反馈环节，分析原系统的开环控制性能。于是系统框图变为如图$\ref{3}$所示：

\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.95\textwidth]{figure/Fig3.png}
	\caption{系统开环控制框图}
	\label{3}
\end{figure*}

开环传递函数为：

\begin{equation}
\frac{X(s)}{U(s)}=
	\frac{
		K_aK_{sv}
	}{
		sA_1
		(\displaystyle \frac{s^2}{\omega_v^2}+\frac{2\xi_vs}{\omega_v}+1)
		(\frac{s^2}{\omega_h^2}+\frac{2\xi_hs}{\omega_h}+1)
	}
\end{equation}

\chapter{机器视觉}
\section{概述}
\section{相机标定}
\section{Apriltag原理}
(1)Apriltag库提供常用的基准标记物如图$\ref{4}$所示。将标记物放置在目标物上或其附近，目标参考系以标记物中心为坐标原点，标记物所在平面为$x-y$平面。相机将拍到标记物的照片，传入图像处理器中。

\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.95\textwidth]{figure/Fig4.png}
	\caption{基准标记物种类}
	\label{4}
\end{figure*}

(2)对图像进行灰度化，为提高后续边缘检测的速度，先对图像进行降采样，并辅之以高斯滤波，滤除高频噪声。

(3)通过“canny”算子或自适应阈值对图像进行阈值分割，然后根据梯度检测图像中的各种边缘。

(4)运用边缘结构分析查找图像中的四边形，再利用多边形凸包寻找算法进行筛选。

(5)对二维码进行编码与解码\\

\chapter{最小二乘算法}

gsl库求解非线性最小二乘问题的默认方法是信任区域法，下面对其原理进行介绍。

假设我们有一个模型L，在当前迭代点x附近，该模型可以被近似为函数F形式的二阶泰勒展开式，则我们有：
\begin{equation}
F(x+h)\approx L(h)=
F(x)+h^Tc+\frac{1}{2}h^TBh
\end{equation}

此处我们用h替代变量，即当前迭代中需要在初值x上增加的量。但要使这个近似假设成立是有一定条件的，我们可以设立一个正值变量$\Delta$使得模型在一个以x为圆心为半径的圆中被视为可以精确近似。这个圆就是我们所说的信任区域。然后我们就可以基于以下公式求解变化值h：
\begin{equation}
h=
{argmin}_{||h||\leq \Delta}[L(h)]
\end{equation}

该公式表示，在信任区域半径限制内，求解出最优的变化值使得模型L的值最小。但是在计算过程中，可信任区域半径值是会不停改变的，其改变的依据为模型的近似质量，模型的近似质量又可以通过增益比例进行判定。首先我们定义增益比例的概念。增益比例为实际函数改变量和模型该变量之间的差异，数学表达式为：
\begin{equation}
\rho=
\frac{F(x)-F(x+h)}{L(0)-L(h)}
\end{equation}

分子部分表示实际改变量，分母部分表示模型改变量。当得到增益比例后我们就可以通过它去实时控制可信任区域半径以及迭代的步长：
\begin{equation}
\begin{cases}
\Delta=\frac{1}{2}\Delta \qquad (\rho < 0.25)\\
\Delta=\max{\{\Delta, 3h\}} \qquad (\rho > 0.75)
\end{cases} 
\end{equation}

如果增益比例过小(例如小于0.25)，说明实际改变的量远远小于近似模型的值，此时近似情况并不理想，因此我们需要缩小近似范围。反之如果比例较大，说明实际下降的比预期的更大，近似情况比较好，我们可以适当放大一下近似范围。

\chapter{机械臂运动控制实验方案}

\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.6\textwidth]{figure/Fig5.png}
	\caption{控制对象机械臂}  
	\label{5}
\end{figure*}

如图$\ref{5}$所示，被控对象为HLK-HD6W型液压机械臂。该机械臂共有5个关节，其中3个为由液压缸驱动的摆动关节，2个为由液压马达驱动的旋转关节。

现在我们想象有这样一个典型的机械臂作业场景：我们需要操纵机械臂移动到阀门附近，使机械臂夹爪能抓住阀门的开关转动一定角度，实现对阀门的开启或关闭。在这个作业场景中，机械臂的控制目标可拆分为两个，一是控制机械臂的前四个关节，将机械臂末端夹爪移动到阀门开关前，且夹爪朝向正对阀门开关；二是控制机械臂腕部的关节，使末端夹爪能转动指定的角度。我们将在实验中分别实现这两个功能目标来验证控制器的有效性。

\section{夹爪移动实验}
机械臂末端夹爪的空间位姿由关节1-4的夹角决定，对机械臂末端夹爪的位姿控制，实则 是控制各个关节的转角。基于这种想法，可设计出最为简单的一种控制回路，如下图所示。

\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.75\textwidth]{figure/Fig14.png}
	\caption{简单控制回路}
	\label{29}
\end{figure*}

对于这个控制回路，有两个问题需要解决：如何得到关节转角反馈值$\bm{\theta}_{cur}$？如何获取关节转角的目标值$\bm{\theta}_{tar}$？针对这两个问题，下面进行具体阐述。

\subsection{视觉反馈}
由于缺乏角度传感器，机械臂无法实时反馈当前各关节的夹角，只能依靠视觉系统来不断获取运动过程中的位姿信息，借助运动学计算来对夹角求解。根据前面正向运动学的内容可知，机械臂末端执行器位姿与各关节夹角之间存在如下关系：
\begin{equation}
^w\bm{T}_t=
	\bm{T}_1\ \bm{T}_2\ \bm{T}_3\ \bm{T}_4=
		\bm{T}_{\theta}(\theta_1,\theta_2,\theta_3,\theta_4)
\label{30}
\end{equation}

$^w\bm{T}_t$表示机械臂基参考系“$w$”到机械臂末端参考系“$t$”的变换矩阵，$\bm{T}_i$表示根据D-H法建立的由机械臂关节$i$参考系到关节$i+1$参考系的变换矩阵。

这样只要得到$^w\bm{T}_t$，我们便可以通过运动学逆解求得各个关节角。下面我们来讨论如何得到$^w\bm{T}_t$。根据齐次变换的原则，我们把$^w\bm{T}_t$作如下拆分：
\begin{equation}
	^w\bm{T}_t=
		{^w\bm{T}_{a}}{^a\bm{T}_c}{^c\bm{T}_{t}}
\label{31}
\end{equation}

其中“$a$”代表目标参考系，以标记物中心为坐标系原点，标记物平面为x-y平面。“$c$”代表相机参考系。

我们在机械臂关节4和关节5之间的连杆上安装相机，将该相机命名为相机A(与之后的机械臂外部的相机相区别)，相机与末端夹爪的相对位姿固定不变，相机参考系与机械臂末端参考系之间的变换矩阵在实验前测量得到。直接测量误差较大，可通过建立可靠的CAD模型(夹爪、相机、机械臂等)来获得足够精确的$^c\bm{T}_{t}$。

用Apriltag码作为标记物粘贴在目标位置上，调用Apriltag识别算法，可藉由输入识别的Apriltag码类型和相机内参矩阵来得到目标坐标系到相机坐标系的旋转矩阵$^c\bm{R}_a$和位移矩阵$^c\bm{t}_a$。则有
\begin{equation}
	^{c}\bm{T}_{a}=
		\begin{bmatrix} 
			^c\bm{R}_a & ^c\bm{t}_a \\ \bm{0} & 1 
		\end{bmatrix}
\label{32}
\end{equation}

获得$^w\bm{T}_{a}$的过程稍微麻烦一些，我们在机械臂外的固定位置安装相机B。假定在实验过程中机械臂的基参考系是固定不变的(相当于全局参考系)，机械臂基参考系“$w$”与相机B参考系“$c'$”这两个固定参考系之间的变换矩阵$^w\bm{T}_{c'}$可以采用和$^c\bm{T}_{t}$一样的方式直接测得。建立可靠的CAD模型同样有助于降低该部分误差。

相机B对标记物拍照，借助Apriltag视觉识别算法得到$^{c'}\bm{T}_{a}$。这样我们便可进一步求得$^w\bm{T}_{a}$。
\begin{equation}
^w\bm{T}_{a}=
	{^w\bm{T}_{c'}}{^{c'}\bm{T}_{a}}
\label{33}
\end{equation}

在本文所涉及的实验中，标记物即机械臂最终的作用对象(阀门)是静止的，这意味着$^w\bm{T}_{a}$不发生变化。我们只需在每次实验前,面对不同的目标物，进行一次$^{c'}\bm{T}_{a}$测量。

结合式$\ref{30}$、式$\ref{31}$、式$\ref{32}$和式$\ref{33}$，有：
\begin{equation}
	\bm{T}_{\theta}(\theta_1,\theta_2,\theta_3,\theta_4)=
		{^w\bm{T}_{c'}}{^{c'}\bm{T}_{a}}{^{a}\bm{T}_{c}}{^c\bm{T}_{t}}
\label{34}
\end{equation}

式中$^w\bm{T}_{c'}$、$^{c'}\bm{T}_{a}$、$^c\bm{T}_{t}$实验前测量得到，保持不变。实验中，通过相机A不断获取${^{c'}\bm{T}_{a}}$来更新$\bm{T}_{\theta}$。对计算出的$\bm{T}_{\theta}$进行运动学逆解，得到当前各关节转角的反馈值。控制回路如下所示。
\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.8\textwidth]{figure/Fig24.png}
	\caption{完善后的控制回路}
	\label{35}
\end{figure*}

\subsection{逆运动学计算替代方案}
然而，由于$\bm{T}_{\theta}$是通过一系列齐次变换间接得到的，任何来自$^w\bm{T}_{c'}$、$^{c'}\bm{T}_{a}$、$^{a}\bm{T}_{c}$、$^c\bm{T}_{t}$的误差，都会在式$\ref{34}$中累积，增加$\bm{T}_{\theta}$的误差，最终导致运动学逆解失败。运动学逆解时，计算出的雅克比矩阵中同时存在位移量和旋转量。运动学逆解需要对这样的雅克比矩阵求逆，意味着将不同量纲的数值进行直接运算，有违物理准则。

一种解决办法是用空间中三个不同点的坐标来描述机械臂末端夹爪的6个自由度，每个空间点的三维坐标包含三个自由度，三点共面提供三个约束，剩下$3 \times 3-3=6$个自由度。这样根据6个自由度计算出来的雅克比矩阵中全部为位移量，量纲保持一致。

选取Apriltag码的三个角点作为识别对象，假定$2l$为标记物边长，在目标参考下，角点坐标已知：
$$(X_{a1},Y_{a1},Z_{a1})=(-l,l,0)$$
$$(X_{a2},Y_{a2},Z_{a2})=(l,l,0)$$
$$(X_{a3},Y_{a3},Z_{a3})=(-l,-l,0)$$
$$(X_{a4},Y_{a4},Z_{a4})=(l,-l,0)$$

取其中3个角点，将它们的齐次坐标用如下矩阵表示：
$$
\bm{f}_a=\left[\begin{array}{ccc}
			X_{a1} & X_{a2} & X_{a3}\\ 
			Y_{a1} & Y_{a2} & Y_{a3}\\ 
			Z_{a1} & Z_{a2} & Z_{a3}\\  
			1 & 1 & 1 \\			 
		\end{array}\right]
$$

通过一系列齐次变换，我们可推导出这些角点在机械臂基参考系下的坐标：
\begin{equation}
	\bm{f}_w(\bm{\theta})=
		{^w\bm{T}_{t}}{^t\bm{T}_{c}}{^{c}\bm{T}_{a}}\bm{f_a}
\label{36}
\end{equation}
\begin{equation}
	\bm{f}_w=
		{^w\bm{T}_{c'}}{^{c'}\bm{T}_{a}}\bm{f_a}
\label{37}							
\end{equation}

$\bm{f}_w(\bm{\theta})$代表模型预测值，是关于关节转角$(\theta_1,\theta_2,\theta_3,\theta_4)$的函数矩阵；$\bm{f}_w$代表观测值，是常数矩阵。用$f_{ij}(\bm{\theta})$、$f_{ji}$分别代表$\bm{f}_w(\bm{\theta})$、$\bm{f}_w$中第i行、第j列元素。构造如下损失函数：
\begin{equation}
	F(\bm{\theta})=
		\frac{1}{2}\sum_{i=1}^3\sum_{j=1}^3[f_{ij}(\bm{\theta})-f_{ij}]^2
\label{38}
\end{equation}

运用最小二乘法可找出一组$(\theta_1,\theta_2,\theta_3,\theta_4)$使得$F(\bm{\theta})$最小。我们把这一组关节夹角值作为当前机械臂末端夹爪位姿所对应的各关节转角。

求解式$\ref{38}$所示的非线性最小二乘问题可通过调用科学计算库gsl中的相关算法实现。改进后的控制回路如下图所示：
\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.8\textwidth]{figure/Fig25.png}
	\caption{改进后的控制回路}
	\label{39}
\end{figure*}

\subsection{关节角目标值获取}
标记物在机械臂基参考系下的坐标已知，但仅凭这一点我们还无法得到使机械臂移动到目标位置所需的各关节转角。本实验中，我们的目标是把机械臂末端夹爪移动到标记物正前方某一位置处(夹爪不接触标记物)。无论标记物在机械臂基参考系下的位姿如何变化，我们最终想要达到的夹爪与标记物之间的相对位姿$(^t\bm{T}_a)_{tar}$是不变的。因此我们可以通过“演示”的方式来记录下$(^t\bm{T}_a)_{tar}$。

实验前，机械臂不动，把标记物移动到机械臂末端夹爪面前的适当位置，使夹爪和标记物之间的相对位姿满足作业要求。用相机A对当前位置的标记物拍照，调用用Apriltag识别算法得到$(^c\bm{T}_a)_{tar}$。手眼关系$^t\bm{T}_c$已测得，则有：
\begin{equation}
	(^t\bm{T}_a)_{tar}=
		{^t\bm{T}_c} (^c\bm{T}_a)_{tar}
\label{40}
\end{equation}

接着我们便可构造出关于各关节转角目标值的模型预测值以及损失函数
\begin{equation}
	\bm{f}_w(\bm{\theta}_{tar})=
		{^w\bm{T}_t}(\bm{\theta}_{tar})(^t\bm{T}_a)_{tar}\bm{f}_a		
\label{41}
\end{equation}
\begin{equation}
	F(\bm{\theta})=
		\frac{1}{2}\sum_{i=1}^3\sum_{j=1}^3[f_{ij}(\bm{\theta}_{tar})-f_{ij}]	
\label{42}
\end{equation}

用最小二乘法求解式$\ref{42}$，即可得到$\bm{\theta}_{tar}$。最终的控制回路如下所示。
\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.9\textwidth]{figure/Fig13.png}
	\caption{最终的控制回路}
	\label{43}
\end{figure*}

\subsection{单关节实验方案}
为降低液压机械臂运动控制问题的复杂性，本文拟从最简单的单个机械臂关节入手，验证控制方法的可行性。在此基础上再扩展到对机械臂所有关节的控制。

选择机械臂大臂(关节4、5之间的连杆)作为被控对象。实验目标是使机械臂大臂绕关节4转动指定角度。假定关节1、2、3、5固定不动，仅关节4运动，我们可以把机械臂简化为如图$\ref{44}$所示。
\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.8\textwidth]{figure/Fig6.png}
	\caption{简化后的机械臂模型}
	\label{44}
\end{figure*}

关节4的角度是唯一自变量，损失函数简化为：
\begin{equation}
	F(\theta_4)=
		\frac{1}{2}\sum_{i=1}^3\sum_{j=1}^3[f_{ij}(\theta_4)-f_{ij}]
\label{45}	
\end{equation}

\section{腕部转动实验}
与前4个关节的控制方案类似，腕部转动的基本控制流程如图$\ref{29}$所示。然而，由于腕部关节位于相机与末端夹爪之间，腕部的转角值无法像前文夹爪移动实验里那样通过相机识别远处标记物的方式来直接获取，需要采用其他手段来测量。

在不考虑额外安装角度传感器的前提下(会破坏机械臂结构紧凑性)，一种解决思路是在腕部固定一个旋转环，如图$\ref{46}$所示。在旋转环上设计特定图案，使视觉系统能够识别出腕部转动不同角度时旋转环图案的某些几何信息，并计算出对应的腕部转角。
\begin{figure*}[h]
	\centering
	\includegraphics[width=0.4\textwidth]{figure/Fig8.png}
	\caption{机械臂腕部关节}  
	\label{46}
\end{figure*}

\subsection{“单螺旋线”式旋转环图案}
郑志恒等人在旋转环上设计出单根螺旋线样式的图案用于视觉识别，如图$\ref{48}$所示。
旋转环上印有外圆线(通常与旋转环外轮廓重合)，内圆线和螺旋线。内圆线和外圆线为同心圆且圆心位于机械臂末端手爪旋转轴的旋转中心。螺旋线的起点位于外圆线与内圆线之间，螺旋线的中点位于外圆线上，并且起点和终点的连线穿过同心圆的圆心。

假定旋转环圆心为点$O$，螺旋线起点为点$P$，终点为点$S$。以点$O$为坐标原点建立$x-y$轴。首先定义点$P$、点$S$落在$y$轴正半轴时为初始位置，此时腕部转角为0度，如图$\ref{48}(a)$所示。令初始位置$y$轴与内圆线的交点为点$T$。

实验前，将腕部旋转至初始位置，获取相机拍摄的初始位置图像，计算线段$SP$和$PT$的像素长度，记作$|SP|$、$|PT|$。

将腕部旋转任意角度，如图$\ref{48}(b)$所示(以转动90度为例)，旋转环也随之旋转相同角度。获取相机拍摄的图像(图中虚线矩形框内部分)。令此时$y$轴与内圆线、螺旋线、外圆线的交点分别为点$N$、点$Q$、点$M$。计算计算线段$QN$的像素长度，记作$|QN|$。

腕部转角的计算公式如下所示：
\begin{equation} 
\theta=
	\frac{|QN|-|PT|}{|SP|}\cdot 2\pi 
\label{47}
\end{equation}

\begin{figure}[htb]
	\centering
	\begin{subfigure}{0.35\textwidth}
		\includegraphics[width=1\textwidth]{figure/Fig9.png}
		\caption{初始位置}
	\end{subfigure}
	\qquad \qquad
	\begin{subfigure}{0.37\textwidth}
		\includegraphics[width=1\textwidth]{figure/Fig10.png}
		\caption{待测位置}
	\end{subfigure}
	\centering
	\caption{“单螺旋线”式旋转环图案} \label{48}
\end{figure}

该方法简单易实现，但也存在一些问题。通常情况下，安装在机械臂上的相机，除了要识别腕部旋转环外，还要观察末端夹爪的情况，这就使得旋转环的尺寸不能设计得太大(否则会遮挡相机视野)。旋转环图像一般只占相机拍摄的整张图像里的一小部分，如图$\ref{49}$所示。受到图像分辨率的限制，当腕部转动一个很小的角度，转动前后图像识别出的$|QN|$不变，此时该方法无法测量出转角大小。无法测量出的转角最大值代表了该测量方法的测量精度。由于郑志恒等人的方法识别的对象为两点之间的的像素长度，图像分辨率不足加剧了离散二值化图像中$|QN|$变化不连续的情况，将导致测量精度难以保证。

\begin{figure*}[h]
	\centering
	\includegraphics[width=0.4\textwidth]{figure/Fig11.jpg}
	\caption{相机拍摄的图像}  
	\label{49}
\end{figure*}

\subsection{“双偏心圆”式旋转环图案}
在郑志恒等人方法的基础上，考虑采用两个偏心圆组成的填充区域代替螺旋线。改进后的旋转环图案包含外圆线(通常与旋转环外轮廓重合)和由大小两个偏心圆组成的填充区域，如图$\ref{50}$所示。外圆线圆心点$O$位于机械臂末端手爪卡钳旋转轴的旋转中心。

两个偏心圆的绘制流程如图$\ref{51}$所示。首先在外圆线内绘制以点$O$为圆心的大小两个同心圆。将大圆圆心水平向左平移至点A处，将小圆圆心水平向右平移至点B处，保证圆$A$位于外圆线内，圆$B$位于圆$A$内，且点$O$到点$A$、点$B$的距离不同。再把圆$A$、圆$B$竖直向上平移相同距离，两圆最终平移后的圆心分别记作点$A'$、点$B'$。最后将偏心圆$A'$、圆$B'$之间的区域涂黑，便得到我们所需的旋转环图案。

\begin{figure*}[h]
	\centering
	\includegraphics[width=0.3\textwidth]{figure/Fig12.png}
	\caption{“双偏心圆”式旋转环图案}  
	\label{50}
\end{figure*}

\begin{figure}[htb]
	\centering
	\begin{subfigure}{0.15\textwidth}
		\includegraphics[width=1.2\textwidth]{figure/Fig15.png}
		\caption{}
	\end{subfigure}
	\qquad \qquad
	\begin{subfigure}{0.15\textwidth}
		\includegraphics[width=1.2\textwidth]{figure/Fig16.png}
		\caption{}
	\end{subfigure}
	\qquad \qquad
	\begin{subfigure}{0.15\textwidth}
		\includegraphics[width=1.2\textwidth]{figure/Fig17.png}
		\caption{}
	\end{subfigure}
	\qquad \qquad
	\begin{subfigure}{0.15\textwidth}
		\includegraphics[width=1.2\textwidth]{figure/Fig18.png}
		\caption{}
	\end{subfigure}
	\centering
	\caption{旋转环图案绘制过程} \label{51}
\end{figure}

下面来推导“双偏心圆”式旋转环的腕部转角计算公式。两个偏心圆中，大圆的半径为$r_A$。假定腕部转角为$\theta$，如图$\ref{52}(a)$所示，点$P$为$y$轴和大圆的交点，作$A'Q\perp OP$于点$Q$。令$\angle{A'OA}={\theta}_A$，有：

\begin{equation} 
\angle{A'OQ}=
	\theta-{\theta}_A+\frac{\pi}{2}
\label{53}
\end{equation}

当$|A'B|$远小于$|OP|$时:
\begin{equation} 
|QP|\approx|A'P|=r_A 
\label{54}
\end{equation}

在三角形$A'PO$中：
\begin{equation} 
|OP|=
	|QP|+|OQ|=
		r_A+|OA'|\cdot \cos(\theta-{\theta}_A+\frac{\pi}{2})  
\label{55}
\end{equation} 

\begin{figure}[htb]
	\centering
	\begin{subfigure}{0.3\textwidth}
		\includegraphics[width=1.3\textwidth]{figure/Fig19.png}
		\caption{}
	\end{subfigure}
	\qquad \qquad 
	\begin{subfigure}{0.3\textwidth}
		\includegraphics[width=1.3\textwidth]{figure/Fig20.png}
		\caption{}
	\end{subfigure}
	\centering
	\caption{转角计算公式推导} \label{52}
\end{figure}

类似的，圆C半径为$r_C$，点$P'$为小圆与$y$轴的交点，作${B'Q'}\perp{OP}'$于点$Q'$，如图$\ref{52}(b)$所示。令$\angle{B'OB}={\theta}_B$，则有：
\begin{equation} 
|OP'|=
	|Q'P'|+|OQ'|=
		r_B+|OB'|\cdot \cos(\theta-{\theta}_B+\frac{\pi}{2})
\label{56}
\end{equation}

旋转环一旦设计完成，式中的$r_A$、$r_B$、$|0A'|$、$|OB'|$、$\theta_A$、$\theta_B$便都是已知且固定不变的。又由于$0<\theta_A,\theta_B<\pi/2$，且$\theta_A\neq \theta_B$，只要测量出$|OP|$和$|OP'|$，便可根据式$\ref{55}$、式$\ref{56}$计算出当前位置的腕部转角$\theta$。

为减轻像素长度变化不连续对测量精度的影响，使用“双偏心圆”式旋转环测量腕部转角时，我们通常选择用视觉识别特定区域的面积和重心纵坐标，来替代识别像素长度$|OP|$、$|OP'|$。相机拍摄到的旋转环图像如图$\ref{57}$所示。
\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.4\textwidth]{figure/Fig21.jpg}
	\caption{相机拍摄的原图像}  \label{57}
\end{figure*}

由于存在安装误差，相机光轴与旋转环平面的交点未必在$y$轴上(注意此处是指图$\ref{52}$中以旋转环圆心为原点建立的$y$轴，而非图像参考系或是像素参考系中的$y$轴)，这就使得$y$轴在图$\ref{57}$所示图像中的位置未知。为找到$y$轴位置，对原图像进行滤波(去除噪声)、二值化操作后，截取出只包含旋转环填充区域的那一部分图像片段，如图$\ref{58}$所示。其中$(x_1,y_u)$、$(x_2,y_u)$、$(x_1,y_d)$、 $(x_2,y_d)$为图$\ref{58}$四个角点在图$\ref{57}$像素参考系下的坐标。
\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.6\textwidth]{figure/Fig22.png}
	\caption{只包含旋转环填充区域的二值图像片段}  \label{58}
\end{figure*}

从左往右、从右往左分别对图$\ref{58}$所下边缘进行遍历，找出黑白分界点，$y$轴位于这两个点的中点处。进一步，在图$\ref{58}$中截取出以$y$轴对对称轴，宽度为$n$个像素点的矩形区域，如图$\ref{59}$所示。
\begin{figure*}[htb]
	\centering
	\includegraphics[width=0.05\textwidth]{figure/Fig23.png}
	\caption{待识别的矩形区域}  \label{59}
\end{figure*}

记图$\ref{59}$中黑色区域的面积为$S$，在图$\ref{59}$像素参考系下的重心纵坐标为$Y$。当n很小的时候，存在以下近似关系：
\begin{equation} 
\frac{S}{n}\approx 
	|PP'|=
		(|OP'|-|OP|)
\label{60}
\end{equation}	

\begin{equation} 
Y\approx 
	\frac{\cdot (|OP'|+|OP|)}{2}-y_u
\label{61}
\end{equation}		

结合式\ref{55}、式\ref{56}和三角函数万能公式，上述两个式子可进一步写成：
\begin{equation} 
S= 
	[R_1 +\sqrt{{M_1}^2+{N_1}^2}\sin{(\theta+\arctan{\frac{N_1}{M_1}}})]\cdot n 
\label{62}
\end{equation}	

\begin{equation} 
Y= 
	R_2 +\sqrt{{M_2}^2+{N_2}^2}\sin{(\theta+\arctan{\frac{N_2}{M_2}}}) 
\label{63}
\end{equation}

其中:
$$ R_1=r_A-r_B$$
$$R_2=\frac{\cdot (r_A+r_B)}{2}-y_u$$
$$M_1=|OA'|\cos{\theta_A}-|OB'|\cos{\theta_B}$$
$$N_1=|OA'|\sin{\theta_A}-|OB'|\sin{\theta_B}$$
$$M_2=\frac{|OA'|\cos{\theta_A}+|OB'|\cos{\theta_B}}{2}$$
$$N_2=\frac{|OA'|\sin{\theta_A}+|OB'|\sin{\theta_B}}{2}$$

上述参数均为常数，且$\displaystyle-\frac{\pi}{2}<\arctan{\frac{N_1}{M_1}},\arctan{\frac{N_2}{M_2}}<\frac{\pi}{2}$，故通过一组$S$、$Y$值可确定唯一一个$\theta$值。这样做的好处是:识别与面积相关的图像特征(重心纵坐标也是与面积相关的，因为它需要通过面积矩求得)，等效于同时识别$n$列像素长度。当腕部转角很小的时候，只要这$n$列像素长度中的任意一列发生了变化，这个转角便是可测得的，大大提高了测量精度。当然，$n$的取值也并非越大越好，若$n$取得过大，式$\ref{60}$和式$\ref{61}$不再满足，测量误差反而会更大。


\chapter{测试结果及分析}
\section{通过gsl最小二乘法求解各关节夹角}
\subsection{概述}
在上一章节“夹爪移动实验”中，我们将最小二乘法的目标函数定义为：
\begin{equation*}
F(\bm{\theta})=
	\frac{1}{2}\sum_{i=1}^3\sum_{j=1}^3[f_{ij}(\bm{\theta})-f_{ij}]^2
\end{equation*}

其中$f_{ij}(\bm{\theta})$、$f_{ji}$分别代表模型预测值$\bm{f}_w(\bm{\theta})$、观测值$\bm{f}_w$中第i行、第j列元素。$\bm{f}_w(\bm{\theta})$、$\bm{f}_w$的表达式如下：

\begin{equation*}
\bm{f}_w(\bm{\theta})=
{^w\bm{T}_{t}}{^t\bm{T}_{c}}{^{c}\bm{T}_{a}}\bm{f_a}
\end{equation*}
\begin{equation*}
\bm{f}_w=
{^w\bm{T}_{a}}\bm{f_a}						
\end{equation*}

本阶段测试中，使机械臂静止，机械臂与标记物的相对位姿固定不变。上述参数中${^w\bm{T}_{t}}$、${^t\bm{T}_{c}}$、${^w\bm{T}_{a}}$通过人工直接测量的方式(直尺，激光测距仪)得到，${^{c}\bm{T}_{a}}$通过视觉识别得到。根据上述参数求出目标函数与雅可比矩阵(gsl最小二乘算法中要用)进行最小二乘求解，将计算出的各关节夹角与机械臂实际关节夹角进行比较。

\subsection{人工测量}
实验前，使用Matlab对相机进行标定，得到相机内参矩阵：
\begin{equation*} 
CameraParams =
	\begin{bmatrix} 
		2054 & 0 & 631 \\
		0 & 2051 & 422 \\
		0 & 0 & 1 
	\end{bmatrix}
\end{equation*}

将相机安装在机械臂关节4、5之间的连杆上，如图$\ref{64}$所示:
\begin{figure}[htb]
	\centering
	\includegraphics[width=0.7\textwidth]{figure/Fig26.jpg}
	\caption{相机安装位置}  \label{64}
\end{figure}

测量相机参考系$c$到机械臂末端参考系$t$的变换矩阵：
\begin{equation*} 
^t\bm{T}_{c}=
	\begin{bmatrix} 
		1 & 0 & 0 & 0 \\
		0 & 1 & 0 & -87 \\
		0 & 0 & 1 & 243 \\
		0 & 0 & 0 & 1
	\end{bmatrix}
\end{equation*}

标记物摆放的位置如图$\ref{65}$所示：
\begin{figure} 
	\centering
	\includegraphics[width=0.7\textwidth]{figure/Fig27.jpg}
	\caption{相机与标记物的相对位置}  \label{65}
\end{figure}

测量机械臂基参考系$w$到标记物参考系$a$的变换矩阵：
\begin{equation*} 
^w\bm{T}_a=
	\begin{bmatrix} 
		1 & 0 & 0 & -13.7 \\
		0 & 1 & 0 & 39.3 \\
		0 & 0 & 1 & 172.5 \\
		0 & 0 & 0 & 1
	\end{bmatrix}
\end{equation*}

标记物边长4cm，3个角点在标记物参考系(以标记物中心位坐标原点)下的齐次坐标为：
\begin{equation*} 
\bm{f}_a = 
	\begin{bmatrix} 
		-20 & 20 & -20 \\
		20 & 20 & -20 \\
		0 & 0 & 0 \\
		1 & 1 & 1
	\end{bmatrix}
\end{equation*}


\subsection{视觉识别}
相机对标记物拍照，对图像灰度化处理后，使用apriltag算法识别标记物四个角点在像素参考系下的坐标：
\begin{equation*} 
2d-points = 
	\begin{bmatrix} 
		apriltag.corners[0] \\
		apriltag.corners[1] \\
		apriltag.corners[2] \\
		apriltag.corners[3]
	\end{bmatrix}
\end{equation*}

标记物四个角点在标记物参考系下的坐标已知(为了与二维点相对应，按照“左上、右上、右下、左下”的顺序排列)：
\begin{equation*} 
3d-points =
	\begin{bmatrix} 
		-20 & -20 & 0 \\
		-20 & 20 & 0 \\
		20 & 20 & 0 \\
		20 & -20 & 0
	\end{bmatrix}
\end{equation*}

调用cv2.solvePnP()函数，输入二维点坐标$2d-points$、三维点坐标$3d-points$、相机内参$CameraParams$和相机畸变系数(此处假定相机无畸变)，得到旋转矢量$rvec$和位移矢量$tvec$，则标记物参考系$a$到相机参考系$c$的变换矩阵：
\begin{equation*} 
^a\bm{T}_c = 
	\begin{bmatrix} 
		rvec & tvec \\
		\bm{0} & 1 
	\end{bmatrix}
\end{equation*}

求逆后得到到相机参考系$c$到标记物参考系$a$的变换矩阵：
\begin{equation*} 
^c\bm{T}_a = {^a\bm{T}_c}^{-1} 
\end{equation*}

备注：键入“python3 -m apriltag -help”出现的说明文档很短，没有找到Linux版本apriltag算法如何直接输出变换矩阵(还请谢老师指点一下)。经验证，使用上述PnP算法计算出的变换矩阵与我在windows系统下使用apriltag库计算出来的变换矩阵一致。
\subsection{计算目标函数与雅可比矩阵}
建立正向运动学模型，测量得到各DH参数：
\begin{equation*} 
	\centering
	\includegraphics[width=1\textwidth]{figure/Fig28.png}
\end{equation*}

\begin{equation*} 
	\centering
	\includegraphics[width=0.5\textwidth]{figure/Fig29.png}
\end{equation*}

根据DH参数，关节$i-1(i=1,2\cdots5)$参考系到关节$i$参考系的变换矩阵可表示为：
\begin{equation}
T_i = 
	\begin{bmatrix} 
		\cos{\theta_i} & \sin{\theta_i} & 0 & a_i \\
		\sin{\theta_i}\cdot \cos{\alpha_i} & \cos{\theta_i}\cdot \cos{\alpha_i} & -\sin{\alpha_i} & -\sin{\alpha_i} \cdot d_i \\
		\sin{\theta_i}\cdot \sin{\alpha_i} & \cos{\theta_i}\cdot \sin{\alpha_i} & \cos{\alpha_i} & \cos{\alpha_i} \cdot d_i \\
		0 & 0 & 0 & 1
	\end{bmatrix}
\label{65}
\end{equation}

将所有关节间的变换矩阵按顺序相乘，即得到机械臂基参考系(关节0参考系)$w$到机械臂末端参考系$t$的变换矩阵：
\begin{equation}
^w\bm{T}_t=
	\bm{T}_1\ \bm{T}_2\ \bm{T}_3\ \bm{T}_4\ \bm{T}_5
\label{66}
\end{equation}

进一步可求得标记物3个角点在机械臂基参考系下的坐标估计值：
\begin{equation}
\bm{f}_{est} = 
	{^w\bm{T}_t} \bm{f}_t
\label{67}
\end{equation}

雅克比矩阵是$\bm{f}_{est}$对各关节夹角求偏导。以$\theta_1$为例，由于$\bm{f}_{est}$可分解为一系列$T_i(i=1,2\cdots5)$的乘积，根据矩阵乘法的结合律：
\begin{equation}
\bm{f}_{est} = 
	\bm{T}_1\ \bm{T}_2\ \bm{T}_3\ \bm{T}_4\ \bm{T}_5\ \bm{f}_t =
		\bm{T}_1\ \hat{\bm{T}_1}
\label{68}
\end{equation}

上式中仅有$\bm{T}_1$含有$\theta_1$，则有：
\begin{equation}
\frac{\partial \bm{f}_{est}}{\partial \theta_1} =
	\frac{\mathrm{d} \bm{T}_1}{\mathrm{d} \theta_1} \cdot \hat{\bm{T}_1}
\label{69}
\end{equation}

根据$\bm{T_i}$的表达式，很容易推导出$\bm{T_i}$对$\theta_i$求导后的结果：
\begin{equation}
\frac{\mathrm{d} \bm{T}_i}{\mathrm{d} \theta_i} = 
	\begin{bmatrix} 
		-\sin{\theta_i} & \cos{\theta_i} & 0 & 0 \\
		\cos{\theta_i}\cdot \cos{\alpha_i} & -\sin{\theta_i}\cdot \cos{\alpha_i} & 0 & 0 \\
		\cos{\theta_i}\cdot \sin{\alpha_i} & -\sin{\theta_i}\cdot \sin{\alpha_i} & 0 & 0 \\
		0 & 0 & 0 & 0
	\end{bmatrix}
\label{70}
\end{equation}

因此，$\bm{f}_{est}$对$\theta_i$求偏导时，只需在式$\ref{68}$中用$\displaystyle \frac{\mathrm{d} \bm{T}_i}{\mathrm{d} \theta_i}$替换$\bm{T_i}$即可。最后将求出的偏导值按照一定顺序组合得到雅可比矩阵$J$。

\subsection{测试结果及分析}
仅移动关节1，给出机械臂在两种不同姿态下，相机拍摄到的标记物图像：
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig30.jpg}
\end{equation*}
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig31.jpg}
\end{equation*}

针对第一张图像，关节角初值选取$[90,0,80,90,10]$，计算结果如下所示：
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig32.jpg}
\end{equation*}

针对第二张图像，关节角初值选取$[90,0,80,90,10]$，计算结果如下所示：
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig33.jpg}
\end{equation*}

针对第一张图像，关节角初值选取$[90,0,90,90,10]$，计算结果如下所示：
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig34.jpg}
\end{equation*}

针对第二张图像，关节角初值选取$[90,0,90,90,10]$，计算结果如下所示：
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig35.jpg}
\end{equation*}

针对第一张图像，关节角初值选取$[90,0,91,90,10]$，计算结果如下所示：
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig36.jpg}
\end{equation*}

针对第一张图像，关节角初值选取$[90,0,89,90,10]$，计算结果如下所示：
\begin{equation*}
\includegraphics[width=0.7\textwidth]{figure/Fig37.jpg}
\end{equation*}

存在的问题：

1.当前没有直接测量关节夹角的手段，对于测量结果究竟准不准，只能依靠肉眼判断，难以给出一个精度范围。

2.对比1、2组，3、4组测试结果，仅仅使一个关节夹角发生变化，其他关节角保持不变，但计算结果中所有关节夹角均有不同程度的变化。

3.对比1、3、5、6组测试结果，关节角保持不变，仅改变迭代初值，计算结果中个别角度存在较大变化。

详细代码参见“gsl-test.py”




























\end{document}































	
